import rclpy
from rclpy.node import Node

import math
from tkinter import END
import numpy as np

from geometry_msgs.msg import Pose
from carsource_interfaces.msg import GPSInterface

class UwbPosNode(Node):
    def __init__(self, name):
        super.__init__(name)

       
        self.pos = np.array([0,0])  #uwb的初始预测位姿 也是最终算出来的位姿
        
        self.B1= 0.1 # 迭代最小值
        
        self.sv_pose = np.array([])
        self.sv_num = 4
        
        
        self.current_uwb_pos_pub = self.create_publisher(Pose, "gps_data",1)

    def config_input(self, base_station):
        #输入基站的数量
        self.sv_pose = base_station

    def run(self,anchor):

        END_LOOP=100 #结束标志
        max_retry = 5
        lpos = self.pos
        while END_LOOP > 0.1 and max_retry > 0:
        
            x_y = self.sv_pose - self.pos
            # while END_LOOP > B1 and max_retry > 0:   

            r = []

            for i in range(x_y.shape[0]):
                r.append(math.sqrt(math.pow(x_y[i][0],2) + math.pow(x_y[i][1],2)))

            # print("test_anchor ",test_anchor)

            r = np.array(r)
            dp = (anchor - r)

            H = []
            
            for i in range(x_y.shape[0]):
                H.append(x_y[i]/r[i])

            H = np.array(H)

            H = -H

            # np.matmul 矩阵运算

            delta = np.matmul(np.matmul(np.linalg.inv((np.matmul(H.transpose(),H))) , H.transpose()),dp)
            
            self.pos = self.pos + delta
            END_LOOP = np.linalg.norm(delta)
            max_retry = max_retry -1
            if(END_LOOP > 10 and max_retry == 0):
                print("uwb失败")
                self.pos = lpos


        msg = GPSInterface()
 
        time_stamp = self.get_clock().now().to_msg()
        cur_time = time_stamp.sec + time_stamp.nanosec/1e9
        
        msg.time = cur_time
        msg.frame_id = "gps"
        msg.x_pos = self.pos[0]
        msg.y_pos = self.pos[1]
        msg.anchor = anchor.tolist()
        msg.base_station = self.sv_pose.tolist()

        self.current_uwb_pos_pub.publish(msg)

def main(args=None):


    rclpy.init(args=args)
    uwb_node = UwbPosNode("uwb_pose_node")


    base_station = np.array([])
    while(rclpy.ok()):
        
        # rclpy.spin_once(uwb_node)
        #TODO 接受uwb信号

        uwb_node.config_input(base_station)

        uwb_node.run()

    uwb_node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()